diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 61a61ab..85918e8 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -307,7 +307,7 @@
 	volatile struct qTD *vtd;
 	unsigned long ts;
 	uint32_t *tdp;
-	uint32_t endpt, maxpacket, token, usbsts;
+	uint32_t endpt, maxpacket, token, usbsts, qhtoken;
 	uint32_t c, toggle;
 	uint32_t cmd;
 	int timeout;
@@ -551,22 +551,21 @@
 	flush_dcache_range((unsigned long)qtd,
 			   ALIGN_END_ADDR(struct qTD, qtd, qtd_count));
 
-	/* Set async. queue head pointer. */
-	ehci_writel(&ctrl->hcor->or_asynclistaddr, virt_to_phys(&ctrl->qh_list));
-
 	usbsts = ehci_readl(&ctrl->hcor->or_usbsts);
 	ehci_writel(&ctrl->hcor->or_usbsts, (usbsts & 0x3f));
 
 	/* Enable async. schedule. */
 	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
-	cmd |= CMD_ASE;
-	ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
+	if (!(cmd & CMD_ASE)) {
+		cmd |= CMD_ASE;
+		ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
 
-	ret = handshake((uint32_t *)&ctrl->hcor->or_usbsts, STS_ASS, STS_ASS,
-			100 * 1000);
-	if (ret < 0) {
-		printf("EHCI fail timeout STS_ASS set\n");
-		goto fail;
+		ret = handshake((uint32_t *)&ctrl->hcor->or_usbsts, STS_ASS, STS_ASS,
+				100 * 1000);
+		if (ret < 0) {
+			printf("EHCI fail timeout STS_ASS set\n");
+			goto fail;
+		}
 	}
 
 	/* Wait for TDs to be processed. */
@@ -587,6 +586,11 @@
 			break;
 		WATCHDOG_RESET();
 	} while (get_timer(ts) < timeout);
+	qhtoken = hc32_to_cpu(qh->qh_overlay.qt_token);
+
+	ctrl->qh_list.qh_link = cpu_to_hc32(virt_to_phys(&ctrl->qh_list) | QH_LINK_TYPE_QH);
+	flush_dcache_range((unsigned long)&ctrl->qh_list,
+		ALIGN_END_ADDR(struct QH, &ctrl->qh_list, 1));
 
 	/*
 	 * Invalidate the memory area occupied by buffer
@@ -605,25 +609,12 @@
 	if (QT_TOKEN_GET_STATUS(token) & QT_TOKEN_STATUS_ACTIVE)
 		printf("EHCI timed out on TD - token=%#x\n", token);
 
-	/* Disable async schedule. */
-	cmd = ehci_readl(&ctrl->hcor->or_usbcmd);
-	cmd &= ~CMD_ASE;
-	ehci_writel(&ctrl->hcor->or_usbcmd, cmd);
-
-	ret = handshake((uint32_t *)&ctrl->hcor->or_usbsts, STS_ASS, 0,
-			100 * 1000);
-	if (ret < 0) {
-		printf("EHCI fail timeout STS_ASS reset\n");
-		goto fail;
-	}
-
-	token = hc32_to_cpu(qh->qh_overlay.qt_token);
-	if (!(QT_TOKEN_GET_STATUS(token) & QT_TOKEN_STATUS_ACTIVE)) {
-		debug("TOKEN=%#x\n", token);
-		switch (QT_TOKEN_GET_STATUS(token) &
+	if (!(QT_TOKEN_GET_STATUS(qhtoken) & QT_TOKEN_STATUS_ACTIVE)) {
+		debug("TOKEN=%#x\n", qhtoken);
+		switch (QT_TOKEN_GET_STATUS(qhtoken) &
 			~(QT_TOKEN_STATUS_SPLITXSTATE | QT_TOKEN_STATUS_PERR)) {
 		case 0:
-			toggle = QT_TOKEN_GET_DT(token);
+			toggle = QT_TOKEN_GET_DT(qhtoken);
 			usb_settoggle(dev, usb_pipeendpoint(pipe),
 				       usb_pipeout(pipe), toggle);
 			dev->status = 0;
@@ -641,11 +632,11 @@
 			break;
 		default:
 			dev->status = USB_ST_CRC_ERR;
-			if (QT_TOKEN_GET_STATUS(token) & QT_TOKEN_STATUS_HALTED)
+			if (QT_TOKEN_GET_STATUS(qhtoken) & QT_TOKEN_STATUS_HALTED)
 				dev->status |= USB_ST_STALLED;
 			break;
 		}
-		dev->act_len = length - QT_TOKEN_GET_TOTALBYTES(token);
+		dev->act_len = length - QT_TOKEN_GET_TOTALBYTES(qhtoken);
 	} else {
 		dev->act_len = 0;
 #ifndef CONFIG_USB_EHCI_FARADAY